package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Kinect;
import edu.wpi.first.wpilibj.KinectStick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import java.util.TimerTask;
import org.nashua.tt151.dashboard.Dashboard;

public class Main extends SimpleRobot{

    CANJaguar fr,fl,br,bl,pickup;
    Jaguar topShoot,lowShoot;
    Servo shootYaw,shootPitch,dropper;
    DigitalOutput led = new DigitalOutput(1);
    Dashboard dash;
    public void robotInit(){
        fr=Drive.createJag(RobotMap.JAGUAR_FRONT_RIGHT);
        fl=Drive.createJag(RobotMap.JAGUAR_FRONT_LEFT);
        br=Drive.createJag(RobotMap.JAGUAR_BACK_RIGHT);
        bl=Drive.createJag(RobotMap.JAGUAR_BACK_LEFT);
        pickup=Drive.createJag(RobotMap.JAGUAR_PICKUP);
        topShoot=new Jaguar(RobotMap.JAGUAR_SHOOTER_TOP);
        lowShoot=new Jaguar(RobotMap.JAGUAR_SHOOTER_BOTTOM);

        dropper=new Servo(RobotMap.SERVO_DROPPER);
        shootYaw=new Servo(RobotMap.SERVO_YAW);
        shootPitch=new Servo(RobotMap.SERVO_PITCH);

        /* Try To Establish Connection To ToughTechsDashboard */
        try {
            dash = new Dashboard(this);
            /* Update ToughTechsDashboard Periodically */
            new java.util.Timer().schedule(new TimerTask(){
                public void run() {
                    while (true) {
                        try {
                            if (fl!=null) updateCAN(RobotMap.JAGUAR_FRONT_LEFT,"Front Left",fl);
                            if (fr!=null) updateCAN(RobotMap.JAGUAR_FRONT_RIGHT,"Front Right",fr);
                            if (bl!=null) updateCAN(RobotMap.JAGUAR_BACK_LEFT,"Back Left",bl);
                            if (br!=null) updateCAN(RobotMap.JAGUAR_BACK_RIGHT,"Back Right",br);
//                            if (topShoot!=null) updateCAN(RobotMap.JAGUAR_SHOOTER_TOP,"Shooter Top",topShoot);
//                            if (lowShoot!=null) updateCAN(RobotMap.JAGUAR_SHOOTER_BOTTOM,"Shooter Bottom",lowShoot);
                            if (pickup!=null) updateCAN(RobotMap.JAGUAR_PICKUP,"Pickup",pickup);
                            if (dropper!=null) dash.updateInfo("Servo Dropper",""+format(dropper.get(),2));
                            if (shootPitch!=null) dash.updateInfo("Servo Pitch",""+format(shootPitch.get(),2));
                            if (shootYaw!=null) dash.updateInfo("Servo Yaw",""+format(shootYaw.get(),2));
                            Timer.delay(0.1);
                        } catch (Exception e) {
                            //Log.log("WARNING: Cannot Update ToughTechsDashboard - "+e.getMessage());
                        }
                    }
                }
                public String format(double num, double dec){
                    return ""+((int)num*(dec=MathUtils.pow(10,dec)))/dec;
                }
                public void updateCAN(int i, String name, CANJaguar jag){
                    try {
                        dash.updateCAN(i, name, format(jag.getX(),2)+"%", format(jag.getBusVoltage(),2)+"V", format(jag.getOutputVoltage(),2)+"V");
                    } catch (CANTimeoutException e) {
                        dash.updateCAN(i, name, "CAN TIMEOUT", "CAN TIMEOUT", "CAN TIMEOUT");
                    } catch (Exception e) {
                        //Log.log("Warning: Cannot Update CAN Jaguar '"+name+"'");
                    }
                }
            }, 1);
        } catch (Exception e) {
            //Log.log("Error: Failed To Establish Connection To ToughTechsDashboard - "+e.getMessage());
        }
    }
    public void autonomous(){
        Kinect kin=Kinect.getInstance();
        KinectStick leftStick=new KinectStick(RobotMap.KINECT_LEFT_ARM);
        KinectStick rightStick=new KinectStick(RobotMap.KINECT_RIGHT_ARM);
        double power=0.5,lastArmSet=RobotMap.V_SERVO_DROPPER_DEF;
        boolean inverse=false;
        while(isEnabled()&&isAutonomous()){
            //POWER SET
            power=0.5;
            led.set(true);
            
            if(leftStick.getRawButton(RobotMap.KINECT_HEAD_LEFT))power=0.25;
            if(leftStick.getRawButton(RobotMap.KINECT_HEAD_RIGHT))power=1;
            //PICKUP TOGGLE
            Drive.setJag(pickup,leftStick.getRawButton(RobotMap.KINECT_LEFT_LEG_OUT)?.5:0);
            //DROPPER TOGGLE
            dropper.set(leftStick.getRawButton(RobotMap.KINECT_RIGHT_LEG_OUT)?RobotMap.V_SERVO_DROPPER_DOWN:RobotMap.V_SERVO_DROPPER_DEF);
            //TANK DRIVE
            Drive.setJag(fl,leftStick.getY());
            Drive.setJag(fr,-rightStick.getY());
            Drive.setJag(bl,Drive.readJag(fl));
            Drive.setJag(br,Drive.readJag(fr));
            //SHOOTER
            //if (leftStick.getRawButton(RobotMap.KINECT_LEFT_LEG_FWD)) Drive.shoot(topShoot, lowShoot, Drive.autoAim(shootPitch, shootYaw, 0, led, dash));
            Timer.delay(.05);
        }
    }
    public void operatorControl(){
        Joystick driver=new Joystick(1),shooter=new Joystick(2);
        boolean[][][] bools=new boolean[2][12][2];
        boolean inverse=false;
        double power=0.5,voltage=0,voltageTarget=0;

        dropper.set(RobotMap.V_SERVO_DROPPER_DEF);
        shootPitch.set(RobotMap.V_SERVO_PITCH_DEF);
        //shootYaw.set(RobotMap.V_SERVO_YAW_DEF);

        while(isEnabled()&&isOperatorControl()){
            power=0.5;//POWER SET
            if(driver.getRawButton(RobotMap.DUALACTION_LEFT_TRIGGER))power=0.25;
            if(driver.getRawButton(RobotMap.DUALACTION_RIGHT_TRIGGER))power=1;

            if(shooter.getRawButton(RobotMap.DUALACTION_LEFT_TRIGGER))voltageTarget=0;
            if(shooter.getRawButton(RobotMap.DUALACTION_BUTTON_A))voltageTarget=2;
            if(shooter.getRawButton(RobotMap.DUALACTION_BUTTON_B))voltageTarget=4;
            if(shooter.getRawButton(RobotMap.DUALACTION_BUTTON_X))voltageTarget=6;
            if(shooter.getRawButton(RobotMap.DUALACTION_BUTTON_Y))voltageTarget=8;
            voltage=voltage+(voltageTarget-voltage)/10;
            System.out.println(voltageTarget);
            System.out.println(voltage);

            Drive.manualPickup(driver,1,pickup,bools);//PICKUP TOGGLE DRIVER
            Drive.manualPickup(shooter,1,pickup,bools);//PICKUP TOGGLE SHOOTER
            Drive.manualDrop(driver,dropper);//DROPPER TOGGLE
            Drive.manualDrive(driver,fl,fr,bl,br,power,inverse);//TANK DRIVE
            Drive.manualShooter(voltage,shooter,shootYaw,shootPitch,topShoot,lowShoot);//SHOOTER


            /*if (shooter.getRawButton(RobotMap.DUALACTION_LEFT_TRIGGER)) Drive.shoot(topShoot, lowShoot, 0.5);
            if (shooter.getRawButton(RobotMap.DUALACTION_RIGHT_TRIGGER)) {
                int hoop = 0; // HIGHEST
                double x = shooter.getRawAxis(5);
                double y = shooter.getRawAxis(6);
                if (x<-0.5) hoop = 2; // MIDDLE LEFT
                else if (x>0.5) hoop = 4; // MIDDLE RIGHT
                else if (y<-0.5) hoop = 1; // TOP
                else if (y>0.5) hoop = 5; // BOTTOM
                Drive.manualShooter(shooter, shootYaw, shootPitch, topShoot, lowShoot, hoop, led, dash);
            }*/
            Timer.delay(.05);
        }
    }
}